/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Converts MaCI/LnMlib logfiles into MRPT rawlog format
* uses MPRT -> GPL Licence should be added!!!
*
*/

#include <mrpt/core.h>
#include <mrpt/slam.h>
#include <mrpt/poses.h> 
#include "CScanMeasurementSet.h"
using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::slam;
using namespace mrpt::poses;
 
 void printUsage(){
	 fprintf(stderr,"\n-----------------------------------------------------------------\n");
	 fprintf(stderr,"Converts MaCI/LnMlib logfiles into \n");
	 fprintf(stderr,"Usage: ./MaCI2Rawlog [in_lnmfile]\n");
	 fprintf(stderr,"MACI FILES: ./MaCI2Rawlog [in_position] [in_ranging]\n");
	 fprintf(stderr,"\nThe output is saved to 'dataset.rawlog'\n");
	 fprintf(stderr,"-----------------------------------------------------------------\n\n");
 }
 
 
 int main(int argc, char *argv[])
{
	/// Open MaCI/LnMlib logfiles
	char *position_file_name=NULL;
	char *ranging_file_name=NULL;
	char *measurement_file_name=NULL;
	float THE_BEAM_OFFSET = 0; ///Rawlog needs to know the direction of first beam
	if(argc == 3){
		position_file_name = argv[1];
		ranging_file_name = argv[2];
	}else if(argc == 2){
		measurement_file_name = argv[1];
	}
	else{
		printUsage();
		exit(1);
	}
	
	CScanMeasurementSet *dset;
	
	if(position_file_name != NULL){
		dset = new CScanMeasurementSet(position_file_name,ranging_file_name);
	}else{
		dset = new CScanMeasurementSet(measurement_file_name);
	}
	
	double beamwidth = fabs(dset->angles[0]-dset->angles[1]);
	double apperture = (dset->angles.size()-1.0) * beamwidth;
	bool rightToLeft;
	if((dset->angles[0]-dset->angles[1])<0){
		rightToLeft = true;
	}else{
		rightToLeft = false;
	}
	
	fprintf(stderr,"Beam = %.3lf (%lf deg) Size=%d rightToLeft=%d\n",beamwidth,beamwidth*180.0/M_PI, 
																																	dset->angles.size(),rightToLeft);

	
	fprintf(stderr,"DATASET size = %d Nummeas=%d Apperture = %.2f\n", dset->size(),dset->angles.size(),apperture*180.0/M_PI);
	////////////////////////////////////////////////////////////////////////////////////77
	////////////////////////////////////////////////////////////////////////////////////77
	/// Doing convert
	////////////////////////////////////////////////////////////////////////////////////77
	////////////////////////////////////////////////////////////////////////////////////77
	
	CFileGZOutputStream  f("dataset.rawlog");
	
	Grid::TScan meas;
	Grid::pose lp;
	Grid::pose old=dset->poses[0];
	
	fprintf(stderr,"Sensor Pose = (%.2f,%.2f,%.2f, %.2f)\n",dset->lx,dset->ly,dset->lz, dset->la);
	CPose3D m_sensorPose(dset->lx,dset->ly, dset->lz, dset->la+THE_BEAM_OFFSET,0,0);
	
	for(int i=0;i<dset->size();i++)
	{
		CActionCollection	actions;
		CSensoryFrame		SF;
		//dset->getMeasurementInLaserFrame(i, lp, meas);
		lp.setToDifferentialPose(dset->poses[i],old);
		old = dset->poses[i];
		// Fill out the actions:
		// ----------------------------------
		CActionRobotMovement2D myAction;  // For example, 2D odometry
		CActionRobotMovement2D::TMotionModelOptions opts;
		opts.modelSelection = CActionRobotMovement2D::mmGaussian;
		CPose2D actualOdometryReading(lp.x, lp.y, lp.a );
		myAction.computeFromOdometry( actualOdometryReading, opts );
		
		actions.insert( myAction );
 
		// Fill out the observations:
		// ----------------------------------
		CObservation2DRangeScanPtr  myObs = CObservation2DRangeScanPtr(new CObservation2DRangeScan());
		myObs->rightToLeft = rightToLeft;
		myObs->aperture = apperture;
		myObs->maxRange	= dset->deviceLimits.max_distance-1.0;
		myObs->stdError = 0.003f;
		myObs->sensorPose = m_sensorPose;
		myObs->scan = dset->ranges[i];
		
		myObs->validRange.resize(dset->ranges[i].size());
		
		for (size_t j=0;j<dset->ranges[i].size();j++)
			myObs->validRange[j] = (myObs->scan[j] <= myObs->maxRange);
 
		SF.insert( myObs );  // memory of "myObs" will be automatically freed.
		
		// Save to the rawlog file:
		// --------------------------------
		f << actions << SF;
	}
	
		
	
 
	return 0;
}
